#include "rclcpp/rclcpp.hpp"
// #include "turn_on_wheeltec_robot/Quaternion_Solution.h"

#include <inttypes.h>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <memory>

#include <iostream>
#include <string.h>
#include <string> 
#include <iostream>
#include <math.h> 
#include <stdlib.h>    
#include <unistd.h>      
#include <rcl/types.h>
#include <sys/stat.h>

#include <fcntl.h>          
#include <stdbool.h>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/float32.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
// #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2/LinearMath/Transform.h"
#include "tf2/LinearMath/Quaternion.h"
#include <tf2_ros/transform_broadcaster.h>
// #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"     // CHANGE
// #include "wheeltec_robot_msg/msg/data.hpp"     // CHANGE
// #include "sensor_msgs/msg/imu.hpp"
#include <nav_msgs/msg/odometry.hpp>
#include <iostream>
#include <string.h>
#include <string>
#include <iostream>
#include <math.h>
#include <stdlib.h>
#include <unistd.h>
#include <rcl/types.h>
#include <sys/stat.h>

// 跨平台socket支持
#ifdef _WIN32
    #include <winsock2.h>
    #include <ws2tcpip.h>
    #pragma comment(lib, "ws2_32.lib")
    typedef SOCKET SocketType;
#else
    #include <sys/socket.h>
    #include <netinet/in.h>
    #include <arpa/inet.h>
    #include <unistd.h>
    typedef int SocketType;
    #define INVALID_SOCKET -1
    #define SOCKET_ERROR -1
    #define closesocket close
#endif
using namespace std;

using std::placeholders::_1;
using namespace std;
rclcpp::Node::SharedPtr node_handle = nullptr;
//Macro definition
//宏定义
#define SEND_DATA_CHECK   1          //Send data check flag bits //发送数据校验标志位
#define READ_DATA_CHECK   0          //Receive data to check flag bits //接收数据校验标志位
#define FRAME_HEADER      0X7B       //Frame head //帧头
#define FRAME_TAIL        0X7D       //Frame tail //帧尾
#define RECEIVE_DATA_SIZE 24         //The length of the data sent by the lower computer //下位机发送过来的数据的长度
#define SEND_DATA_SIZE    11         //The length of data sent by ROS to the lower machine //ROS向下位机发送的数据的长度
#define PI 				  3.1415926f //PI //圆周率

//Relative to the range set by the IMU gyroscope, the range is ±500°, corresponding data range is ±32768
//The gyroscope raw data is converted in radian (rad) units, 1/65.5/57.30=0.00026644
//与IMU陀螺仪设置的量程有关，量程±500°，对应数据范围±32768
//陀螺仪原始数据转换位弧度(rad)单位，1/65.5/57.30=0.00026644
#define GYROSCOPE_RATIO   0.00026644f
//Relates to the range set by the IMU accelerometer, range is ±2g, corresponding data range is ±32768
//Accelerometer original data conversion bit m/s^2 units, 32768/2g=32768/19.6=1671.84	
//与IMU加速度计设置的量程有关，量程±2g，对应数据范围±32768
//加速度计原始数据转换位m/s^2单位，32768/2g=32768/19.6=1671.84
#define ACCEl_RATIO 	  1671.84f  	


//Covariance matrix for speedometer topic data for robt_pose_ekf feature pack
//协方差矩阵，用于里程计话题数据，用于robt_pose_ekf功能包
const double odom_pose_covariance[36]   = {1e-3,    0,    0,   0,   0,    0, 
										      0, 1e-3,    0,   0,   0,    0,
										      0,    0,  1e6,   0,   0,    0,
										      0,    0,    0, 1e6,   0,    0,
										      0,    0,    0,   0, 1e6,    0,
										      0,    0,    0,   0,   0,  1e3 };

const double odom_pose_covariance2[36]  = {1e-9,    0,    0,   0,   0,    0, 
										      0, 1e-3, 1e-9,   0,   0,    0,
										      0,    0,  1e6,   0,   0,    0,
										      0,    0,    0, 1e6,   0,    0,
										      0,    0,    0,   0, 1e6,    0,
										      0,    0,    0,   0,   0, 1e-9 };

const double odom_twist_covariance[36]  = {1e-3,    0,    0,   0,   0,    0, 
										      0, 1e-3,    0,   0,   0,    0,
										      0,    0,  1e6,   0,   0,    0,
										      0,    0,    0, 1e6,   0,    0,
										      0,    0,    0,   0, 1e6,    0,
										      0,    0,    0,   0,   0,  1e3 };
										      
const double odom_twist_covariance2[36] = {1e-9,    0,    0,   0,   0,    0, 
										      0, 1e-3, 1e-9,   0,   0,    0,
										      0,    0,  1e6,   0,   0,    0,
										      0,    0,    0, 1e6,   0,    0,
										      0,    0,    0,   0, 1e6,    0,
										      0,    0,    0,   0,   0, 1e-9} ;

//Data structure for speed and position
//速度、位置数据结构体
typedef struct __Vel_Pos_Data_
{
	float X;
	float Y;
	float Z;
}Vel_Pos_Data;


//The structure of the ROS to send data to the down machine
//ROS向下位机发送数据的结构体
typedef struct _SEND_DATA_  
{
	    uint8_t tx[SEND_DATA_SIZE];
		float X_speed;	       
		float Y_speed;           
		float Z_speed;         
		unsigned char Frame_Tail; 
}SEND_DATA;

//The structure in which the lower computer sends data to the ROS
//下位机向ROS发送数据的结构体
typedef struct _RECEIVE_DATA_     
{
	    uint8_t rx[RECEIVE_DATA_SIZE];
	    uint8_t Flag_Stop;
		unsigned char Frame_Header;
		float X_speed;  
		float Y_speed;  
		float Z_speed;  
		float Power_Voltage;	
		unsigned char Frame_Tail;
}RECEIVE_DATA;

//The robot chassis class uses constructors to initialize data, publish topics, etc
//机器人底盘类，使用构造函数初始化数据和发布话题等
class turn_on_robot : public rclcpp::Node

{
	public:
		turn_on_robot();
		~turn_on_robot(); //Destructor //析构函数
		void Control();   //Loop control code //循环控制代码
                void update_tfupdate_tf(geometry_msgs::msg::TransformStamped::SharedPtr odom_tf);
		void Publish_Odom();      //Pub the speedometer topic //发布里程计话题
		// serial::Serial Stm32_Serial; //Declare a serial object //声明串口对象 
		//explicit turn_on_robot(
		 // const std::string & name = "wheeltec_robot");
         // 读取传感器数据
         // 计算校验和
        uint8_t calculateChecksum(const uint8_t* data, size_t len)
        {
            uint8_t sum = 0;
            for (size_t i = 0; i < len; i++) {
                sum ^= data[i];
            }
            return sum;
        }
        // Socket初始化
        bool initSocket()
        {
        #ifdef _WIN32
            WSADATA wsaData;
            if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
                RCLCPP_ERROR(get_logger(), "Failed to initialize WinSock");
                return false;
            }
        #endif

            udp_socket_ = socket(AF_INET, SOCK_DGRAM, 0);
            if (udp_socket_ == INVALID_SOCKET) {
                RCLCPP_ERROR(get_logger(), "Failed to create socket: %s", strerror(errno));
                return false;
            }

            // 设置接收超时时间（可选）
            struct timeval tv;
            tv.tv_sec = 10;  // 10秒超时
            tv.tv_usec = 0;
            if (setsockopt(udp_socket_, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof(tv)) < 0) {
                RCLCPP_WARN(get_logger(), "Failed to set receive timeout");
            }

            // 设置本地地址
            memset(&local_addr_, 0, sizeof(local_addr_));
            local_addr_.sin_family = AF_INET;
            local_addr_.sin_addr.s_addr = htonl(INADDR_ANY);
            local_addr_.sin_port = htons(local_port_);

            // 绑定本地地址
            if (bind(udp_socket_, (struct sockaddr*)&local_addr_, sizeof(local_addr_)) == SOCKET_ERROR) {
                RCLCPP_ERROR(get_logger(), "Failed to bind socket: %s", strerror(errno));
                return false;
            }

            // 设置远程地址
            memset(&remote_addr_, 0, sizeof(remote_addr_));
            remote_addr_.sin_family = AF_INET;
            remote_addr_.sin_port = htons(remote_port_);
            if (inet_pton(AF_INET, remote_ip_.c_str(), &remote_addr_.sin_addr) <= 0) {
                RCLCPP_ERROR(get_logger(), "Invalid remote IP address: %s", remote_ip_.c_str());
                return false;
            }

            RCLCPP_INFO(get_logger(), "UDP socket initialized - Local port: %d, Remote: %s:%d",
                        local_port_, remote_ip_.c_str(), remote_port_);
            return true;
        }

        // 发送数据
        bool sendData(const uint8_t* data, size_t len)
        {
            RCLCPP_DEBUG(get_logger(), "Sending data to %s:%d", remote_ip_.c_str(), remote_port_);
            ssize_t sent = sendto(udp_socket_, (const char*)data, len, 0,
                                (struct sockaddr*)&remote_addr_, sizeof(remote_addr_));
            if (sent < 0) {
                RCLCPP_ERROR(get_logger(), "Failed to send data: %s", strerror(errno));
                return false;
            }
            return static_cast<size_t>(sent) == len;
        }

        // 接收数据
        bool receiveData(uint8_t* data, size_t len)
        {
            struct sockaddr_in sender_addr;
            socklen_t sender_addr_len = sizeof(sender_addr);
            
        #ifdef _WIN32
            int received = recvfrom(udp_socket_, (char*)data, len, 0,
                                (struct sockaddr*)&sender_addr, &sender_addr_len);
        #else
            // 使用阻塞式接收，简化代码
            ssize_t received = recvfrom(udp_socket_, data, len, 0,
                                    (struct sockaddr*)&sender_addr, &sender_addr_len);
        #endif

            if (received < 0) {
                RCLCPP_ERROR(get_logger(), "Failed to receive data: %s", strerror(errno));
                return false;
            }
            
            // 打印接收到的数据（十六进制格式）
            std::stringstream hex_stream;
            for (size_t i = 0; i < static_cast<size_t>(received); i++) {
                hex_stream << std::hex << std::setw(2) << std::setfill('0') 
                        << static_cast<int>(data[i]) << " ";
            }
            RCLCPP_DEBUG(get_logger(), "Received %zd bytes: %s", 
                        received, hex_stream.str().c_str());
            
            return true;
        }

        // 关闭Socket
        void closeSocket()
        {
            if (udp_socket_ != INVALID_SOCKET) {
                closesocket(udp_socket_);
            }
        #ifdef _WIN32
            WSACleanup();
        #endif
        }

	private:

		//rclcpp::Node::SharedPtr nl;           //Create a ROS node handle //创建ROS节点句柄
		rclcpp::Time _Now, _Last_Time;  //Time dependent, used for integration to find displacement (mileage) //时间相关，用于积分求位移(里程)
		float Sampling_Time;         //Sampling time, used for integration to find displacement (mileage) //采样时间，用于积分求位移(里程)
		//rclcpp::Subscriber Cmd_Vel_Sub; //Initialize the topic subscriber //初始化话题订阅者
		//Ackerman speed topic subscribe callback function, type Ackerman trolleys use this function declaration
		//阿克曼速度话题订阅回调函数，类型阿克曼小车使用此函数声明
		//The speed topic subscribes to the callback function, which is used by other cars to declare
		//速度话题订阅回调函数，其它小车使用此函数声明
		//void Cmd_Vel_Callback(const geometry_msgs::Twist &twist_aux);       

  		rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr Cmd_Vel_Sub;
  		// rclcpp::Subscription<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr Akm_Cmd_Vel_Sub;

                rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher;         // CHANGE
                rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr voltage_publisher;         // CHANGE
                // rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher;         // CHANGE

                rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr test_publisher;         // CHANGE

                // rclcpp::Publisher<wheeltec_robot_msg::msg::Data>::SharedPtr robotpose_publisher;         // CHANGE
                // rclcpp::Publisher<wheeltec_robot_msg::msg::Data>::SharedPtr robotvel_publisher;         // CHANGE
                rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr tf_pub_;

                std::shared_ptr<tf2_ros::TransformBroadcaster> tf_bro;
                rclcpp::TimerBase::SharedPtr test_timer;

                rclcpp::TimerBase::SharedPtr odom_timer;
                // rclcpp::TimerBase::SharedPtr imu_timer;
                rclcpp::TimerBase::SharedPtr voltage_timer;

                rclcpp::TimerBase::SharedPtr robotpose_timer;
                rclcpp::TimerBase::SharedPtr robotvel_timer;

                std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

                void declare_parameters();
                void get_parameters();


                void Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux);
                // void Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl);
                //void Akm_Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux);
		auto createQuaternionMsgFromYaw(double yaw);
 
        //从串口(ttyUSB)读取运动底盘速度、IMU、电源电压数据
        //Read motion chassis speed, IMU, power supply voltage data from serial port (ttyUSB)
        bool Get_Sensor_Data();   
        unsigned char Check_Sum(unsigned char Count_Number,unsigned char mode); //BBC check function //BBC校验函数
        short IMU_Trans(uint8_t Data_High,uint8_t Data_Low);  //IMU data conversion read //IMU数据转化读取
		float Odom_Trans(uint8_t Data_High,uint8_t Data_Low); //Odometer data is converted to read //里程计数据转化读取

        string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test; //Define the related variables //定义相关变量
        std::string cmd_vel;
        int serial_baud_rate;      //Serial communication baud rate //串口通信波特率
        RECEIVE_DATA Receive_Data; //The serial port receives the data structure //串口接收数据结构体
        SEND_DATA Send_Data;       //The serial port sends the data structure //串口发送数据结构体

        Vel_Pos_Data Robot_Pos;    //The position of the robot //机器人的位置
        Vel_Pos_Data Robot_Vel;    //The speed of the robot //机器人的速度

        bool socket_initialized_;
        SocketType udp_socket_;
        struct sockaddr_in remote_addr_;    // 远程地址
        struct sockaddr_in local_addr_;     // 本地地址
        std::string remote_ip_;             // 远程IP
        int remote_port_;                   // 远程端口
        int local_port_;                    // 本地端口

        // 数据缓冲区
        uint8_t send_buffer_[SEND_DATA_SIZE];
        uint8_t receive_buffer_[RECEIVE_DATA_SIZE];
    size_t count_;
};

/**************************************
Date: January 28, 2021
Function: The main function, ROS initialization, creates the Robot_control object through the Turn_on_robot class and automatically calls the constructor initialization
功能: 主函数，ROS初始化，通过turn_on_robot类创建Robot_control对象并自动调用构造函数初始化
***************************************/

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    //auto node= std::make_shared<turn_on_robot>();

    turn_on_robot Robot_Control;
    Robot_Control.Control();
    return 0;
}
/**************************************
Date: January 28, 2021
Function: Data conversion function
功能: 数据转换函数
***************************************/

float turn_on_robot::Odom_Trans(uint8_t Data_High,uint8_t Data_Low)
{
  float data_return;
  short transition_16;
  transition_16 = 0;
  transition_16 |=  Data_High<<8;  //Get the high 8 bits of data   //获取数据的高8位
  transition_16 |=  Data_Low;      //Get the lowest 8 bits of data //获取数据的低8位
  data_return   =  (transition_16 / 1000)+(transition_16 % 1000)*0.001; // The speed unit is changed from mm/s to m/s //速度单位从mm/s转换为m/s
  return data_return;
}


//void turn_on_robot::Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl) 
void turn_on_robot::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux)
{
  short  transition;  //intermediate variable //中间变量
  //if(akm_cmd_vel=="none") {RCLCPP_INFO(this->get_logger(),"not akm");} //Prompt message //提示信息
  Send_Data.tx[0]=FRAME_HEADER; //frame head 0x7B //帧头0X7BAkm_Cmd_Vel_Sub
  Send_Data.tx[1] = 0; //set aside //预留位
  Send_Data.tx[2] = 0; //set aside //预留位

  //The target velocity of the X-axis of the robot
  //机器人x轴的目标线速度
  transition=0;
  transition = twist_aux->linear.x*1000; //将浮点数放大一千倍，简化传输
  Send_Data.tx[4] = transition;     //取数据的低8位
  Send_Data.tx[3] = transition>>8;  //取数据的高8位

  //The target velocity of the Y-axis of the robot
  //机器人y轴的目标线速度
  transition=0;
  transition = twist_aux->linear.y*1000;
  Send_Data.tx[6] = transition;
  Send_Data.tx[5] = transition>>8;

  //The target angular velocity of the robot's Z axis
  //机器人z轴的目标角速度
  transition=0;
  transition = twist_aux->angular.z*1000;
  Send_Data.tx[8] = transition;
  Send_Data.tx[7] = transition>>8;

  Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //For the BBC check bits, see the Check_Sum function //BBC校验位，规则参见Check_Sum函数
  Send_Data.tx[10]=FRAME_TAIL; //frame tail 0x7D //帧尾0X7D

  try
  {
    if(akm_cmd_vel=="none")  
    {
        // Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));} //Sends data to the downloader via serial port //通过串口向下位机发送数据 
        sendData(Send_Data.tx, sizeof(Send_Data.tx));
    }
  }
  catch (std::exception& e) {
          RCLCPP_ERROR(this->get_logger(), "Unable to send data through serial port");
    }
  }


/**************************************
Date: January 28, 2021
Function: Publish the odometer topic, Contains position, attitude, triaxial velocity, angular velocity about triaxial, TF parent-child coordinates, and covariance matrix
功能: 发布里程计话题，包含位置、姿态、三轴速度、绕三轴角速度、TF父子坐标、协方差矩阵
***************************************/

void turn_on_robot::Publish_Odom()
{
    //Convert the Z-axis rotation Angle into a quaternion for expression 
     //把Z轴转角转换为四元数进行表达

    tf2::Quaternion q;
    q.setRPY(0,0,Robot_Pos.Z);
    geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q);

    // wheeltec_robot_msg::msg::Data robotpose;
    // wheeltec_robot_msg::msg::Data robotvel;
    nav_msgs::msg::Odometry odom; //Instance the odometer topic data //实例化里程计话题数据

    odom.header.stamp = rclcpp::Node::now(); 
    odom.header.frame_id = odom_frame_id; // Odometer TF parent coordinates //里程计TF父坐标
    odom.child_frame_id = robot_frame_id; // Odometer TF subcoordinates //里程计TF子坐标

    odom.pose.pose.position.x = Robot_Pos.X; //Position //位置
    odom.pose.pose.position.y = Robot_Pos.Y;
    odom.pose.pose.position.z = Robot_Pos.Z;
    odom.pose.pose.orientation = odom_quat; //Posture, Quaternion converted by Z-axis rotation //姿态，通过Z轴转角转换的四元数


    odom.twist.twist.linear.x =  Robot_Vel.X; //Speed in the X direction //X方向速度
    odom.twist.twist.linear.y =  Robot_Vel.Y; //Speed in the Y direction //Y方向速度
    odom.twist.twist.angular.z = Robot_Vel.Z; //Angular velocity around the Z axis //绕Z轴角速度 

    // robotpose.x = Robot_Pos.X;
    // robotpose.y = Robot_Pos.Y;
    // robotpose.z = Robot_Pos.Z;

    // robotvel.x = Robot_Vel.X;
    // robotvel.y = Robot_Vel.Y;
    // robotvel.z = Robot_Vel.Z;

 /*   geometry_msgs::msg::TransformStamped odom_tf;

    odom_tf.header = odom.header;
    odom_tf.child_frame_id = odom.child_frame_id;
    odom_tf.header.stamp = rclcpp::Node::now();

    odom_tf.transform.translation.x = odom.pose.pose.position.x;
    odom_tf.transform.translation.y = odom.pose.pose.position.y;
    odom_tf.transform.translation.z = odom.pose.pose.position.z;
    odom_tf.transform.rotation = odom.pose.pose.orientation;

    tf_bro->sendTransform(odom_tf);

*/
    //There are two types of this matrix, which are used when the robot is at rest 
    //and when it is moving.Extended Kalman Filtering officially provides 2 matrices for the robot_pose_ekf feature pack
    //这个矩阵有两种，分别在机器人静止和运动的时候使用。扩展卡尔曼滤波官方提供的2个矩阵，用于robot_pose_ekf功能包
    //tf_pub_->publish(odom_tf);
    odom_publisher->publish(odom); //Pub odometer topic //发布里程计话题
    // robotpose_publisher->publish(robotpose); //Pub odometer topic //发布里程计话题
    // robotvel_publisher->publish(robotvel); //Pub odometer topic //发布里程计话题

}
/*************************************
Date: January 28, 2021
Function: Serial port communication check function, packet n has a byte, the NTH -1 byte is the check bit, the NTH byte bit frame end.Bit XOR results from byte 1 to byte n-2 are compared with byte n-1, which is a BBC check
Input parameter: Count_Number: Check the first few bytes of the packet
功能: 串口通讯校验函数，数据包n有个字节，第n-1个字节为校验位，第n个字节位帧尾。第1个字节到第n-2个字节数据按位异或的结果与第n-1个字节对比，即为BBC校验
输入参数： Count_Number：数据包前几个字节加入校验   mode：对发送数据还是接收数据进行校验
***************************************/

unsigned char turn_on_robot::Check_Sum(unsigned char Count_Number,unsigned char mode)
{
  unsigned char check_sum=0,k;
  
  if(mode==0) //Receive data mode //接收数据模式
  {
   for(k=0;k<Count_Number;k++)
    {
     check_sum=check_sum^Receive_Data.rx[k]; //By bit or by bit //按位异或
     }
  }
  if(mode==1) //Send data mode //发送数据模式
  {
   for(k=0;k<Count_Number;k++)
    {
     check_sum=check_sum^Send_Data.tx[k]; //By bit or by bit //按位异或
     }
  }
  return check_sum; //Returns the bitwise XOR result //返回按位异或结果
}


/**************************************
Date: January 28, 2021
Function: The serial port reads and verifies the data sent by the lower computer, and then the data is converted to international units
功能: 通过串口读取并校验下位机发送过来的数据，然后数据转换为国际单位
***************************************/

bool turn_on_robot::Get_Sensor_Data()
{ 
  short transition_16=0, j=0, Header_Pos=0, Tail_Pos=0; //Intermediate variable //中间变量
  uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE]={0}; //Temporary variable to save the data of the lower machine //临时变量，保存下位机数据
//   Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr)); //Read the data sent by the lower computer through the serial port //通过串口读取下位机发送过来的数据
    receiveData(Receive_Data_Pr, sizeof(Receive_Data_Pr));
 //Record the position of the head and tail of the frame //记录帧头帧尾位置
  for(j=0;j<24;j++)
  {
    if(Receive_Data_Pr[j]==FRAME_HEADER)
    Header_Pos=j;
    else if(Receive_Data_Pr[j]==FRAME_TAIL)
    Tail_Pos=j;    
  }

  if(Tail_Pos==(Header_Pos+23))
  {
    //If the end of the frame is the last bit of the packet, copy the packet directly to receive_data.rx
    //如果帧尾在数据包最后一位，直接复制数据包到Receive_Data.rx
    // ROS_INFO("1----");
    memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr));
  }
  else if(Header_Pos==(1+Tail_Pos))
  {
    //如果帧头在帧尾后面，纠正数据位置后复制数据包到Receive_Data.rx
    // If the header is behind the end of the frame, copy the packet to receive_data.rx after correcting the data location
    // ROS_INFO("2----");
    for(j=0;j<24;j++)
    Receive_Data.rx[j]=Receive_Data_Pr[(j+Header_Pos)%24];
  }
  else 
  {
    //其它情况则认为数据包有错误
    // In other cases, the packet is considered to be faulty
    // ROS_INFO("3----");
    return false;
  }    

  Receive_Data.Frame_Header= Receive_Data.rx[0]; //The first part of the data is the frame header 0X7B //数据的第一位是帧头0X7B
  Receive_Data.Frame_Tail= Receive_Data.rx[23];  //The last bit of data is frame tail 0X7D //数据的最后一位是帧尾0X7D

  if (Receive_Data.Frame_Header == FRAME_HEADER ) //Judge the frame header //判断帧头
  {
    if (Receive_Data.Frame_Tail == FRAME_TAIL) //Judge the end of the frame //判断帧尾
    { 
      //BBC check passes or two packets are interlaced //BBC校验通过或者两组数据包交错
      if (Receive_Data.rx[22] == Check_Sum(22,READ_DATA_CHECK)||(Header_Pos==(1+Tail_Pos))) 
      {
        Receive_Data.Flag_Stop=Receive_Data.rx[1]; //set aside //预留位
        //Get the speed of the moving chassis in the X direction //获取运动底盘X方向速度
        Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); 
        //Get the speed of the moving chassis in the Y direction, The Y speed is only valid in the omnidirectional mobile robot chassis
        Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); 
                                                                         //获取运动底盘Y方向速度，Y速度仅在全向移动机器人底盘有效
        Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); //Get the speed of the moving chassis in the Z direction //获取运动底盘Z方向速度   

        //Get the battery voltage
        //获取电池电压
        transition_16 = 0;
        transition_16 |=  Receive_Data.rx[20]<<8;
        transition_16 |=  Receive_Data.rx[21];  
        Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; //Unit conversion millivolt(mv)->volt(v) //单位转换毫伏(mv)->伏(v)

        return true;
     }
    }
  } 
  return false;
}
/**************************************
Date: January 28, 2021
Function: Loop access to the lower computer data and issue topics
功能: 循环获取下位机数据与发布话题
***************************************/

void turn_on_robot::Control()
{

  rclcpp::Time current_time, last_time;
  current_time = rclcpp::Node::now();
  last_time = rclcpp::Node::now();
  while(rclcpp::ok())
  {
    try
    {
    current_time = rclcpp::Node::now();
    //Retrieves time interval, which is used to integrate velocity to obtain displacement (mileage) 
    //获取时间间隔，用于积分速度获得位移(里程)
    Sampling_Time = (current_time - last_time).seconds(); 

    //The serial port reads and verifies the data sent by the lower computer, and then the data is converted to international units
    //通过串口读取并校验下位机发送过来的数据，然后数据转换为国际单位
    if (true == Get_Sensor_Data()) 
                                   
    {
      //Calculate the displacement in the X direction, unit: m //计算X方向的位移，单位：m
      Robot_Pos.X+=(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time;
      //Calculate the displacement in the Y direction, unit: m //计算Y方向的位移，单位：m 
      Robot_Pos.Y+=(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time;
      //The angular displacement about the Z axis, in rad //绕Z轴的角位移，单位：rad 
      Robot_Pos.Z+=Robot_Vel.Z * Sampling_Time;

      //Calculate the three-axis attitude from the IMU with the angular velocity around the three-axis and the three-axis acceleration
      //通过IMU绕三轴角速度与三轴加速度计算三轴姿态
    //   Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\
    //             Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z);
      Publish_Odom();

      rclcpp::spin_some(this->get_node_base_interface());

    }

    last_time = current_time; //Record the time and use it to calculate the time interval //记录时间，用于计算时间间隔
    }
    catch (const rclcpp::exceptions::RCLError & e)
    {
      RCLCPP_ERROR(this->get_logger(),"unexpectedly failed with %s",e.what());
    }

    }
}

/**************************************
Date: January 28, 2021
Function: Constructor, executed only once, for initialization
功能: 构造函数, 只执行一次，用于初始化
***************************************/
turn_on_robot::turn_on_robot()
: rclcpp::Node ("wheeltec_robot")
{
  memset(&Robot_Pos, 0, sizeof(Robot_Pos));
  memset(&Robot_Vel, 0, sizeof(Robot_Vel));
  memset(&Receive_Data, 0, sizeof(Receive_Data)); 
  memset(&Send_Data, 0, sizeof(Send_Data));

  int serial_baud_rate = 115200;

  this->declare_parameter<int>("serial_baud_rate");
  this->declare_parameter<std::string>("usart_port_name", "/dev/ttyCH343USB0");
  this->declare_parameter<std::string>("cmd_vel", "cmd_vel");
  this->declare_parameter<std::string>("odom_frame_id", "odom");
  this->declare_parameter<std::string>("robot_frame_id", "base_link");
  this->declare_parameter<std::string>("gyro_frame_id", "gyro_link");

  this->get_parameter("serial_baud_rate", serial_baud_rate);
  this->get_parameter("usart_port_name", usart_port_name);
  this->get_parameter("cmd_vel", cmd_vel);
  this->get_parameter("odom_frame_id", odom_frame_id);
  this->get_parameter("robot_frame_id", robot_frame_id);
  this->get_parameter("gyro_frame_id", gyro_frame_id);

  odom_publisher = create_publisher<nav_msgs::msg::Odometry>("odom", 2);
  //odom_timer = create_wall_timer(1s/50, [=]() { Publish_Odom(); });


  //voltage_timer = create_wall_timer(1s/100, [=]() { Publish_Voltage(); });    
  //tf_pub_ = this->create_publisher<tf2_msgs::msg::TFMessage>("tf", 10);
//   robotpose_publisher = create_publisher<wheeltec_robot_msg::msg::Data>("robotpose", 10);
  //robotpose_timer = create_wall_timer(1s/50, [=]() { Publish_Odom(); });

//   robotvel_publisher = create_publisher<wheeltec_robot_msg::msg::Data>("robotvel", 10);
  //robotvel_timer = create_wall_timer(1s/50, [=]() { Publish_Odom(); });
  tf_bro = std::make_shared<tf2_ros::TransformBroadcaster>(this);

  Cmd_Vel_Sub = create_subscription<geometry_msgs::msg::Twist>(
      cmd_vel, 1, std::bind(&turn_on_robot::Cmd_Vel_Callback, this, _1));

try
{
    initSocket();
}
catch(const std::exception& e)
{
    RCLCPP_ERROR(this->get_logger(),"Failed to initialize UDP communication");
}

}

/**************************************
Date: January 28, 2021
Function: Destructor, executed only once and called by the system when an object ends its life cycle
功能: 析构函数，只执行一次，当对象结束其生命周期时系统会调用这个函数
***************************************/

turn_on_robot::~turn_on_robot()
{
  //Sends the stop motion command to the lower machine before the turn_on_robot object ends
  //对象turn_on_robot结束前向下位机发送停止运动命令
  Send_Data.tx[0]=FRAME_HEADER;
  Send_Data.tx[1] = 0;  
  Send_Data.tx[2] = 0; 

  //The target velocity of the X-axis of the robot //机器人X轴的目标线速度 
  Send_Data.tx[4] = 0;     
  Send_Data.tx[3] = 0;  

  //The target velocity of the Y-axis of the robot //机器人Y轴的目标线速度 
  Send_Data.tx[6] = 0;
  Send_Data.tx[5] = 0;  

  //The target velocity of the Z-axis of the robot //机器人Z轴的目标角速度 
  Send_Data.tx[8] = 0;  
  Send_Data.tx[7] = 0;    
  Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //Check the bits for the Check_Sum function //校验位，规则参见Check_Sum函数
  Send_Data.tx[10]=FRAME_TAIL; 

  try
  {
    // Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Send data to the serial port //向串口发数据  /
    sendData(Send_Data.tx, sizeof(Send_Data.tx));
  }
  catch (std::exception& e)   
  {
    RCLCPP_ERROR(this->get_logger(),"Unable to send data through serial port"); //If sending data fails, an error message is printed //如果发送数据失败,打印错误信息
  }
  // Stm32_Serial.close(); //Close the serial port //关闭串口  
  closeSocket();
  RCLCPP_INFO(this->get_logger(),"Shutting down"); //Prompt message //提示信息
  
}




